#include "MySend.h"



void MySend_task(void *pvParameters)
{ 
	static int Time_count_1=0; //Time variable //计时变量
	u32 lastWakeTime = getSysTickCnt();
	while(1)
	{	
		//此任务以50Hz的频率运行（20ms控制一次）
		vTaskDelayUntil(&lastWakeTime, F2T(RATE_10_HZ));

		//系统启动3秒后再进行控制，避免角度传感器启动故障
		if(Time_count_1>50) 
		{
			CarStatus_Send();
		}
		else
		{
			Time_count_1++;
		}	
	}	
}

void CarStatus_Send(void)
{

 	if(Car_Status != 0)
 	{
		if(flag_S)
			Car_Status = 3;
		else if(Angle[0] - Angle_X >5.0)	
			Car_Status = 2;
 		else if(Angle_X - Angle[0]>5.0)
 			Car_Status = 4;
 		else
 			Car_Status = 1;
		if(click())	Car_Status = 0;
 	}
	else
		if(click())	Car_Status = 1;


//	printf("S_X:%.3f Target_S_X:%.3f\r\n",S_X,Target_S_X);
//	printf("S_Y:%.3f Target_S_Y:%.3f\r\n",S_Y,Target_S_Y);
//	printf("flag_S:%d\r\n",flag_S);	
	//V_X:float V_Y:float Target_Z:float
	//printf("V_X:%.2f V_Y:%.2f Target_Z:%.2f Angle[0]:%.2f Angle[2]:%.2f S:%d\r\n",V_X,V_Y,Target_Z,Angle[0],Angle[2],Car_Status);
	//V_X:float V_Y:float Status:int
	u2_printf("~X%.2f\r\n ~Y%.2f\r\n ~S%d\r\n",V_X,V_Y,Car_Status);
	printf("~X%.2f\r\n ~Y%.2f\r\n ~S%d\r\n",V_X,V_Y,Car_Status);
}

